#include "TwoLayerRungeKutta.h"

#include <iostream>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>

#include "SpinTorque.h"
#include "AloneTorque.h"
#include "NewParameters.h"

int llg3 (double t, const double y[], double f[], void *params)
{

  double *p=(double *)params;
  double alpha1=p[0];
  double gamma1=p[1];
  double alpha2=p[2];
  double gamma2=p[3];

  double beta1_1P=p[4];
  double beta2_1P=p[5];
  double beta1_12=p[6];
  double beta2_12=p[7];
  double beta1_21=p[8];
  double beta2_21=p[9];

  double heffx1=p[10];
  double heffy1=p[11];
  double heffz1=p[12];
  double heffx2=p[13];
  double heffy2=p[14];
  double heffz2=p[15];

  double px=p[16];
  double py=p[17];
  double pz=p[18];

  double mx1=y[0];
  double my1=y[1];
  double mz1=y[2];

  double mx2=y[3];
  double my2=y[4];
  double mz2=y[5];  
  
  // m1 is between the pinned layer p and the free layer m2 
  f[0]=-gamma1*(my1*heffz1-mz1*heffy1)-alpha1*((mx1*mx1-1)*heffx1+mx1*my1*heffy1+mx1*mz1*heffz1)
    +beta1_1P*(my1*pz-mz1*py)-beta2_1P*((mx1*mx1-1)*px+mx1*my1*py+mx1*mz1*pz)
    +beta1_12*(my1*mz2-mz1*my2)-beta2_12*((mx1*mx1-1)*mx2+mx1*my1*my2+mx1*mz1*mz2);
  f[1]=-gamma1*(mz1*heffx1-mx1*heffz1)-alpha1*((my1*my1-1)*heffy1+mz1*my1*heffz1+mx1*my1*heffx1)
    +beta1_1P*(mz1*px-mx1*pz)-beta2_1P*((my1*my1-1)*py+mz1*my1*pz+mx1*my1*px)
    +beta1_12*(mz1*mx2-mx1*mz2)-beta2_12*((my1*my1-1)*my2+mz1*my1*mz2+mx1*my1*mx2);
  f[2]=-gamma1*(mx1*heffy1-my1*heffx1)-alpha1*((mz1*mz1-1)*heffz1+mz1*mx1*heffx1+mz1*my1*heffy1)
    +beta1_1P*(mx1*py-my1*px)-beta2_1P*((mz1*mz1-1)*pz+mz1*mx1*px+mz1*my1*py)
    +beta1_12*(mx1*my2-my1*mx2)-beta2_12*((mz1*mz1-1)*mz2+mz1*mx1*mx2+mz1*my1*my2);  
  //equations for third layer m2
  f[3]=-gamma2*(my2*heffz2-mz2*heffy2)-alpha2*((mx2*mx2-1)*heffx2+mx2*my2*heffy2+mx2*mz2*heffz2)
    +beta1_21*(my2*mz1-mz2*my1)-beta2_21*((mx2*mx2-1)*mx1+mx2*my2*my1+mx2*mz2*mz1);
  f[4]=-gamma2*(mz2*heffx2-mx2*heffz2)-alpha2*((my2*my2-1)*heffy2+mz2*my2*heffz2+mx2*my2*heffx2)
    +beta1_21*(mz2*mx1-mx2*mz1)-beta2_21*((my2*my2-1)*my1+mz2*my2*mz1+mx2*my2*mx1);
  f[5]=-gamma2*(mx2*heffy2-my2*heffx2)-alpha2*((mz2*mz2-1)*heffz2+mz2*mx2*heffx2+mz2*my2*heffy2)
    +beta1_21*(mx2*my1-my2*mx1)-beta2_21*((mz2*mz2-1)*mz1+mz2*mx2*mx1+mz2*my2*my1);
 
  return GSL_SUCCESS;
}

int jac3 (double t, const double y[], double *dfdy, double dfdt[], void *params)
{

  double *p=(double *)params;
  double alpha1=p[0];
  double gamma1=p[1];
  double alpha2=p[2];
  double gamma2=p[3];

  double beta1_1P=p[4];
  double beta2_1P=p[5];
  double beta1_12=p[6];
  double beta2_12=p[7];
  double beta1_21=p[8];
  double beta2_21=p[9];

  double heffx1=p[10];
  double heffy1=p[11];
  double heffz1=p[12];
  double heffx2=p[13];
  double heffy2=p[14];
  double heffz2=p[15];

  double px=p[16];
  double py=p[17];
  double pz=p[18];

  double mx1=y[0];
  double my1=y[1];
  double mz1=y[2];

  double mx2=y[3];
  double my2=y[4];
  double mz2=y[5];   
 
  gsl_matrix_view dfdy_mat 
    = gsl_matrix_view_array (dfdy, 6, 6);
  gsl_matrix * m = &dfdy_mat.matrix; 
  //df[0]
  gsl_matrix_set(m,0,0,alpha1*(2*mx1*heffx1+my1*heffy1+mz1*heffz1)-beta2_1P*(mx1*2*px+my1*py+mz1*pz)
		 -beta2_12*(2*mx1*mx2+my1*my2+mz1*mz2));
  gsl_matrix_set(m,0,1,-gamma1*heffz1-alpha1*+mx1*heffy1+beta1_1P*pz-beta2_1P*+mx1*py+beta1_12*mz2-beta2_12*mx1*my2);
  gsl_matrix_set(m,0,2,gamma1*heffy1-alpha1*+mx1*heffz1-beta1_1P*py-beta2_1P*mx1*pz-beta1_12*my2-beta2_12*mx1*mz2);
  gsl_matrix_set(m,0,3,-beta2_12*(mx1*mx1-1));
  gsl_matrix_set(m,0,4,-beta1_12*mz1-beta2_12*mx1*my1);
  gsl_matrix_set(m,0,5,beta1_12*my1-beta2_12*mx1*mz1);
  //df[1]
  gsl_matrix_set(m,1,0,gamma1*heffz1+alpha1*my1*heffx1-beta1_1P*pz-beta2_1P*my1*px-beta1_12*mz2-beta2_12*my1*mx2);
  gsl_matrix_set(m,1,1,-alpha1*(2*my1*heffy1+mz1*heffz1+mx1*heffx1)-beta2_1P*(2*my1*py+mz1*pz+mx1*px)
		 -beta2_12*(2*my1*my2+mz1*mz2+mx1*mx2));
  gsl_matrix_set(m,1,2,-gamma1*mz1*heffx1-alpha1*my1*heffz1+beta1_1P*px-beta2_1P*my1*pz
		 +beta1_12*mx2-beta2_12*my1*mz2);
  gsl_matrix_set(m,1,3,beta1_12*mz1-beta2_12*mx1*my1);
  gsl_matrix_set(m,1,4,-beta2_12*(my1*my1-1));
 gsl_matrix_set(m,1,5,-beta1_12*mx1-beta2_12*mz1*my1); 
 //df[2]
  gsl_matrix_set(m,2,0,-gamma1*heffy1-alpha1*+mz1*heffx1+beta1_1P*py-beta2_1P*mz1*px+beta1_12*my2-beta2_12*mz1*mx2);
  gsl_matrix_set(m,2,1,gamma1*heffx1-alpha1*mz1*heffy1-beta1_1P*my1*px-beta2_1P*+mz1*py-beta1_12*mx2-beta2_12*mz1*my2);
  gsl_matrix_set(m,2,2,-alpha1*(2*mz1*heffz1+mx1*heffx1+my1*heffy1)-beta2_1P*(2*mz1*pz+mx1*px+my1*py)
		 -beta2_12*(2*mz1*mz2+mx1*mx2+my1*my2));
  gsl_matrix_set(m,2,3,-beta1_12*my1-beta2_12*mz1*mx1);
  gsl_matrix_set(m,2,4,beta1_12*mx1*my2-beta2_12*mz1*my1);
  gsl_matrix_set(m,2,5,-beta2_12*(mz1*mz1-1));
  //df[3]
  gsl_matrix_set(m,3,0,-beta2_21*(mx2*mx2-1));
  gsl_matrix_set(m,3,1,-beta1_21*-mz2-beta2_21*mx2*my2);
  gsl_matrix_set(m,3,2,beta1_21*my2*mz1-beta2_21*mx2*mz2);
  gsl_matrix_set(m,3,3,-alpha2*((2*mx2-1)*heffx2+my2*heffy2+mz2*heffz2)-beta2_21*((2*mx2-1)*mx1+my2*my1+mz2*mz1));
  gsl_matrix_set(m,3,4,-gamma2*heffz2-alpha2*mx2*heffy2+beta1_21*mz1-beta2_21*mx2*my1);
  gsl_matrix_set(m,3,5,gamma2*mz2*heffy2-alpha2*mx2*heffz2-beta1_21*my1-beta2_21*mx2*mz1);
  //df[4]
  gsl_matrix_set(m,4,0,beta1_21*mz2-beta2_21*mx2*my2);
  gsl_matrix_set(m,4,1,-beta2_21*(my2*my2-1));
  gsl_matrix_set(m,4,2,-beta1_21*mx2-beta2_21*mz2*my2);
  gsl_matrix_set(m,4,3,gamma2*heffz2-alpha2*my2*heffx2-beta1_21*mz1-beta2_21*my2*mx1);
  gsl_matrix_set(m,4,4,-alpha2*(2*my2*heffy2+mz2*heffz2+mx2*heffx2)-beta2_21*(2*my2*my1+mz2*mz1+mx2*mx1));
  gsl_matrix_set(m,4,5,-gamma2*mz2*heffx2-alpha2*+my2*heffz2+beta1_21*mx1-beta2_21*my2*mz1);
  //df[5]
 gsl_matrix_set(m,5,0,-beta1_21*my2-beta2_21*mz2*mx2);
  gsl_matrix_set(m,5,1,beta1_21*mx2-beta2_21*mz2*mx2);
  gsl_matrix_set(m,5,2,-beta2_21*(mz2*mz2-1));
  gsl_matrix_set(m,5,3,-gamma2*heffy2-alpha2*mz2*heffx2+beta1_21*my1-beta2_21*mz2*mx1);
 gsl_matrix_set(m,5,4,gamma2*heffx2-alpha2*mz2*heffy2-beta1_21*mx1-beta2_21*mz2*my1);
 gsl_matrix_set(m,5,5,alpha2*(2*mz2*heffz2+mx2*heffx2+my2*heffy2)-beta2_21*(2*mz2*mz1+mx2*mx1+my2*my1));
 
  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  dfdt[2] = 0.0;
  dfdt[3] = 0.0;
  dfdt[4] = 0.0;
  dfdt[5] = 0.0;
 
  return GSL_SUCCESS;
}

double* rungekutta3(Layer mag1,Layer mag2,Layer pol,Torque torque1,Torque torque2,Clock clock,Pulse *current,NewParameters parameters)
{

  Torque storque12,storque21,storque1P;
  storque12=SpinTorque(clock,current,mag1,mag2,parameters); 
  storque21=SpinTorque(clock,current,mag2,mag1,parameters);
  storque1P=SpinTorque(clock,current,mag1,pol,parameters);
  
 double *arr=new double[6];

  const gsl_odeiv_step_type * T 
    = gsl_odeiv_step_rkck;
  
  gsl_odeiv_step * s 
    = gsl_odeiv_step_alloc (T, 6);
  gsl_odeiv_control * c 
    = gsl_odeiv_control_y_new (1E-5, 1);//first is max absolute error second is relative
  gsl_odeiv_evolve * e 
    = gsl_odeiv_evolve_alloc (6);
 
  double alpha1=torque1.getAlpha();
  double gamma1=torque1.getGamma();
  double alpha2=torque2.getAlpha();
  double gamma2=torque2.getGamma();

  double beta1_1P=storque1P.getBeta1();
  double beta2_1P=storque1P.getBeta2();
  double beta1_12=storque12.getBeta1();
  double beta2_12=storque12.getBeta2();
  double beta1_21=storque21.getBeta1();
  double beta2_21=storque21.getBeta2();
 
  double px=pol.getMag().getX();
  double py=pol.getMag().getY();
  double pz=pol.getMag().getZ();

  double heffx1=torque1.getHeff().getX();
  double heffy1=torque1.getHeff().getY();
  double heffz1=torque1.getHeff().getZ();

  double heffx2=torque2.getHeff().getX();
  double heffy2=torque2.getHeff().getY();
  double heffz2=torque2.getHeff().getZ();
       
  double params[19];

  gsl_odeiv_system system = {llg3, jac3, 6, params};

  params[0]=alpha1;
  params[1]=gamma1;
  params[2]=alpha2;
  params[3]=gamma2;
  params[4]=beta1_1P;
  params[5]=beta2_1P;
  params[6]=beta1_12;
  params[7]=beta2_12;
  params[8]=beta1_21;
  params[9]=beta2_21;
  params[10]=heffx1;
  params[11]=heffy1;
  params[12]=heffz1;
  params[13]=heffx2;
  params[14]=heffy2;
  params[15]=heffz2;
  params[16]=px;
  params[17]=py;
  params[18]=pz;
   
  double t = clock.getTimeStamp(), t1 = clock.getTimeStamp()+clock.getTimeStep(); //t=initial time t1=final time
  double h = 1E-10; //initial stepsize
 
  arr[0]=mag1.getMag().getX();
  arr[1]=mag1.getMag().getY();
  arr[2]=mag1.getMag().getZ();
  arr[3]=mag2.getMag().getX();
  arr[4]=mag2.getMag().getY();
  arr[5]=mag2.getMag().getZ();

  while (t < t1)
    {

      int status = gsl_odeiv_evolve_apply (e,c,s,&system,&t,t1,&h,arr);
   
      if (status != GSL_SUCCESS)
	break;
           
      mag1.setMag(arr[0],arr[1],arr[2]);
      mag2.setMag(arr[3],arr[4],arr[5]);
    
    }
  
    gsl_odeiv_evolve_free (e);
    gsl_odeiv_control_free (c);
    gsl_odeiv_step_free (s);

  return arr;
}
